////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//
//
// This file is part of the Microsoft .NET Micro Framework Porting Kit Code Samples and is unsupported.
// Copyright (C) Microsoft Corporation. All rights reserved. Use of this sample source code is subject to
// the terms of the Microsoft license agreement under which you licensed this sample source code.
//
// THIS SAMPLE CODE AND INFORMATION ARE PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND, EITHER EXPRESSED OR IMPLIED,
// INCLUDING BUT NOT LIMITED TO THE IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE.
//
//
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

#include <tinyhal.h>
#include <Cores\arm\Include\cpu.h>

#define SHALL_I_USE_ITCM	1 

static const UINT32  c_ITCMBase					=	0x00100000;
static const UINT32  c_DTCMBase					=	0x00200000;
static const UINT32  c_SectionForAcceleration = 0x20040000;

static const UINT32  c_Bootstrap_Register_Begin =   0xFFF00000;
static const UINT32  c_Bootstrap_Register_End   =   0xFFFFFF00;
static const UINT32  c_Bootstrap_SDRAM_Begin    =   SRAM1_MEMORY_Base;
static const UINT32  c_Bootstrap_SDRAM_End      =   SRAM1_MEMORY_Base + SRAM1_MEMORY_Size;
static const UINT32  c_Bootstrap_SRAM_Begin     =   SRAM_MEMORY_Base;
static const UINT32  c_Bootstrap_SRAM_End       =   SRAM_MEMORY_Base + SRAM_MEMORY_Size;
//static const UINT32  c_Bootstrap_FLASH_Begin    =   FLASH_MEMORY_Base;
//static const UINT32  c_Bootstrap_FLASH_End      =   FLASH_MEMORY_Base + FLASH_MEMORY_Size;
static const UINT32  c_Bootstrap_XR16l78X_Begin =   0x50000000;
static const UINT32  c_Bootstrap_XR16l78X_End   =   0x50000000 + 0x1000;
static const UINT32  c_Bootstrap_AX88796_Begin  =   AX88796_BASE_ADDRESS;
static const UINT32  c_Bootstrap_AX88796_End    =   AX88796_BASE_ADDRESS + 0x1000;

static UINT32* const c_Bootstrap_BaseOfTTBs     =   (UINT32*)(c_Bootstrap_SDRAM_End - ARM9_MMU::c_TTB_size);

extern void AT91_HAG_SdramInit();
extern void AT91_HAG_ClockInit();
extern void HAG_Init();

	// these define the region to zero initialize
extern UINT32 ARM_Vectors;

void MoveTrampolies()
{
	UINT32* src = (UINT32*)&ARM_Vectors;
    UINT32* dst = (UINT32*)NULL;
    UINT32  len = 0x40;

	while (len--) {
		*dst++ = *src++;
	}
}

//  ARM BOOTSTRAPPING FOR ARM9 IS A COMMON METHOD "CPU_ARM9_BootstrapCode".
void BootstrapCode_MMU()
{
    // Fill Translation table with faults.
    ARM9_MMU::InitializeL1(c_Bootstrap_BaseOfTTBs);

    // Direct map for the APB registers (0xFFF00000 ~ 0xFFFFFFFF)
     ARM9_MMU::GenerateL1_Sections(
        c_Bootstrap_BaseOfTTBs,                                 // base of TTBs
        c_Bootstrap_Register_Begin,                             // mapped address
        c_Bootstrap_Register_Begin,                             // physical address
        ARM9_MMU::c_MMU_L1_size,                                // length to be mapped
        ARM9_MMU::c_AP__Manager,                                // AP
        0,                                                      // Domain
        FALSE,                                                  // Cacheable
        FALSE,                                                  // Buffered
        FALSE);                                                 // Extended

    // Direct map SDRAM (cachable)
    ARM9_MMU::GenerateL1_Sections(
        c_Bootstrap_BaseOfTTBs,                                 // base of TTBs
        c_Bootstrap_SDRAM_Begin,                                // mapped address
        c_Bootstrap_SDRAM_Begin,                                // physical address
        c_Bootstrap_SDRAM_End - c_Bootstrap_SDRAM_Begin,        // length to be mapped
        ARM9_MMU::c_AP__Manager,                                // AP
        0,                                                      // Domain
        TRUE,                                                   // Cacheable
        FALSE,                                                  // Buffered
        FALSE);                                                 // Extended

    // Remap SRAM @0x000000000 (cachable)
    ARM9_MMU::GenerateL1_Sections(
        c_Bootstrap_BaseOfTTBs,                                 // base of TTBs
        0x00000000,                                             // mapped address
        c_Bootstrap_SRAM_Begin,                                 // physical address
        ARM9_MMU::c_MMU_L1_size,                                // length to be mapped
        ARM9_MMU::c_AP__Manager,                                // AP
        0,                                                      // Domain
        TRUE,                                                   // Cacheable
        FALSE,                                                  // Buffered
        FALSE);                                                 // Extended

    // Direct map SRAM (cachable)
    ARM9_MMU::GenerateL1_Sections(
        c_Bootstrap_BaseOfTTBs,                                 // base of TTBs
        c_Bootstrap_SRAM_Begin,                                 // mapped address
        c_Bootstrap_SRAM_Begin,                                 // physical address
        c_Bootstrap_SRAM_End - c_Bootstrap_SRAM_Begin,          // length to be mapped
        ARM9_MMU::c_AP__Manager,                                // AP
        0,                                                      // Domain
        TRUE,                                                  // Cacheable
        FALSE,                                                  // Buffered
        FALSE);                                                 // Extended

    // Direct map for the LCD registers(0x00600000~0x006FFFFF)
    	ARM9_MMU::GenerateL1_Sections(
        c_Bootstrap_BaseOfTTBs,                                 // base of TTBs
        AT91C_BASE_LCDC,                                        // mapped address
        AT91C_BASE_LCDC,                                        // physical address
        ARM9_MMU::c_MMU_L1_size,                                // length to be mapped
        ARM9_MMU::c_AP__Manager,                                // AP
        0,                                                      // Domain
        FALSE,                                                  // Cacheable
        FALSE,                                                  // Buffered
        FALSE);                                                 // Extended

    // Direct map for the NAND (uncachable)
    	ARM9_MMU::GenerateL1_Sections(
        c_Bootstrap_BaseOfTTBs,                                 // base of TTBs
        0x40000000,      										// mapped address
        0x40000000,                                				// physical address
        0x10000000,        										// length to be mapped
        ARM9_MMU::c_AP__Manager,                                // AP
        0,                                                      // Domain
        FALSE,                                                  // Cacheable
        FALSE,                                                  // Buffered
        FALSE);                                                 // Extended


    // TODO: UNCOMMENT if FLASH is added on external bus
    //
    // Direct map for the FLASH (cachable)
    //	ARM9_MMU::GenerateL1_Sections(
    //    c_Bootstrap_BaseOfTTBs,                                 // base of TTBs
    //    c_Bootstrap_FLASH_Begin,                                // mapped address
    //    c_Bootstrap_FLASH_Begin,                                // physical address
    //    c_Bootstrap_FLASH_End - c_Bootstrap_FLASH_Begin,        // length to be mapped
    //    ARM9_MMU::c_AP__Manager,                                // AP
    //    0,                                                      // Domain
    //    TRUE,                                                  // Cacheable
    //    FALSE,                                                  // Buffered
    //    FALSE);                                                 // Extended

    // Direct map for the FLASH (uncachable)
    //	ARM9_MMU::GenerateL1_Sections(
    //    c_Bootstrap_BaseOfTTBs,                                 // base of TTBs
    //    CPU_GetUncachableAddress(c_Bootstrap_FLASH_Begin),      // mapped address
    //    c_Bootstrap_FLASH_Begin,                                // physical address
    //    c_Bootstrap_FLASH_End - c_Bootstrap_FLASH_Begin,        // length to be mapped
    //    ARM9_MMU::c_AP__Manager,                                // AP
    //    0,                                                      // Domain
    //    FALSE,                                                  // Cacheable
    //    FALSE,                                                  // Buffered
    //    FALSE);                                                 // Extended
    //

    // Direct map for the UART (uncachable)
    	ARM9_MMU::GenerateL1_Sections(
        c_Bootstrap_BaseOfTTBs,                                 // base of TTBs
        CPU_GetUncachableAddress(c_Bootstrap_XR16l78X_Begin),   // mapped address
        c_Bootstrap_XR16l78X_Begin,                             // physical address
        c_Bootstrap_XR16l78X_End - c_Bootstrap_XR16l78X_Begin,	// length to be mapped
        ARM9_MMU::c_AP__Manager,                                // AP
        0,                                                      // Domain
        FALSE,                                                  // Cacheable
        FALSE,                                                  // Buffered
        FALSE);                                                 // Extended

    // Direct map for the ETH (uncachable)
    	ARM9_MMU::GenerateL1_Sections(
        c_Bootstrap_BaseOfTTBs,                                 // base of TTBs
        CPU_GetUncachableAddress(c_Bootstrap_AX88796_Begin),   	// mapped address
        c_Bootstrap_AX88796_Begin,                             	// physical address
        c_Bootstrap_AX88796_End - c_Bootstrap_AX88796_Begin,	// length to be mapped
        ARM9_MMU::c_AP__Manager,                                // AP
        0,                                                      // Domain
        FALSE,                                                  // Cacheable
        FALSE,                                                  // Buffered
        FALSE);                                                 // Extended


    CPU_FlushCaches();
    CPU_EnableMMU( c_Bootstrap_BaseOfTTBs );
}

static inline void CPU_SetTCMBaseAddress(size_t address, int instr)
{	
	if (instr)
	{
		__asm
		{
			mcr     p15, 0, address, c9, c1, 1
		}
	} 
	else	
	{
		__asm
		{
			mcr     p15, 0, address, c9, c1, 0
		}
	}
}

void SetupITCM()
{
	UINT32 loadaddr = c_SectionForAcceleration & 0xffff0000;

	AT91_MATRIX *matrix = (AT91_MATRIX *)AT91C_BASE_MATRIX;
	
	// activate ITCM
	matrix->MATRIX_TCMR = matrix->MATRIX_ITCM_SIZE_64KB;
	
	// copy code to ITCM
	memcpy((void *)c_ITCMBase,(void *)loadaddr,0x10000);	
	
	// relocate ITCM to SectionForAcceleration 
	CPU_SetTCMBaseAddress(loadaddr|matrix->MATRIX_ITCM_SIZE_64KB<<2|1,1);
}

void BootstrapCode ()
{
	// setup cloch gear
    AT91_HAG_ClockInit();

	// ARM9 specific init code
    CPU_ARM9_BootstrapCode();

	// init SDRAM
    AT91_HAG_SdramInit();

	// Init HOME AUTOMATION GATEWAY
	HAG_Init();

#ifdef SHALL_I_USE_ITCM	
	// setup ITCM BEFORE MMU!
	SetupITCM();
#endif	

	// enable MMU
    BootstrapCode_MMU();

	// init C-Regions
    PrepareImageRegions();

	// copy interrupt vectors to address 0
	MoveTrampolies();
	
	// enable cache
    CPU_EnableCaches();
}
